#!/bin/bash

# Script to test ROS2 vehicle_model package build
set -e

echo "=== Testing ROS2 Vehicle Model Package Build ==="
echo "Current directory: $(pwd)"

# Check if we're in the right workspace
if [ ! -d "vehicle/vehicle_model" ]; then
    echo "Error: vehicle/vehicle_model directory not found. Please run from EPSILON workspace root."
    exit 1
fi

# Source ROS2 environment if available
if [ -f "/opt/ros/galactic/setup.bash" ]; then
    echo "Sourcing ROS2 Galactic..."
    source /opt/ros/galactic/setup.bash
elif [ -f "/opt/ros/galactic/setup.bash" ]; then
    echo "Sourcing ROS2 galactic..."
    source /opt/ros/galactic/setup.bash
elif [ -f "/opt/ros/foxy/setup.bash" ]; then
    echo "Sourcing ROS2 Foxy..."
    source /opt/ros/foxy/setup.bash
else
    echo "Warning: No ROS2 installation found. Continuing anyway..."
fi

# Check dependencies
echo "Checking system dependencies..."
missing_deps=()

# Check for colcon
if ! command -v colcon &> /dev/null; then
    missing_deps+=("python3-colcon-common-extensions")
fi

if [ ${#missing_deps[@]} -ne 0 ]; then
    echo "Missing dependencies: ${missing_deps[*]}"
    echo "Please install them with: sudo apt install ${missing_deps[*]}"
    echo "Continuing anyway to test package structure..."
fi

# Test package structure
echo "=== Checking package structure ==="
required_files=("vehicle/vehicle_model/package.xml" "vehicle/vehicle_model/CMakeLists.txt" "vehicle/vehicle_model/include" "vehicle/vehicle_model/src")
for file in "${required_files[@]}"; do
    if [ -e "$file" ]; then
        echo "✓ Found: $file"
    else
        echo "✗ Missing: $file"
        exit 1
    fi
done

# Check package.xml format
echo "=== Validating package.xml ==="
if xmllint --noout vehicle/vehicle_model/package.xml 2>/dev/null; then
    echo "✓ package.xml is valid XML"
else
    echo "✗ package.xml is invalid XML"
    exit 1
fi

# Test CMake syntax (basic check)
echo "=== Checking CMakeLists.txt syntax ==="
if grep -q "ament_package()" vehicle/vehicle_model/CMakeLists.txt; then
    echo "✓ CMakeLists.txt contains ament_package() - ROS2 format"
else
    echo "✗ CMakeLists.txt missing ament_package() - not ROS2 format"
    exit 1
fi

# Source install/setup.bash if it exists (for common package)
if [ -f "install/setup.bash" ]; then
    echo "Sourcing workspace setup..."
    source install/setup.bash
fi

# Try to build if colcon is available
if command -v colcon &> /dev/null; then
    echo "=== Attempting to build vehicle packages ==="
    echo "Building vehicle_msgs first..."
    
    # Build vehicle_msgs first (dependency)
    if colcon build --packages-select vehicle_msgs --cmake-args -DBUILD_TESTING=OFF; then
        echo "✓ vehicle_msgs builds successfully!"
    else
        echo "✗ vehicle_msgs build failed"
        exit 1
    fi
    
    # Source the newly built packages
    if [ -f "install/setup.bash" ]; then
        source install/setup.bash
    fi
    
    echo "Building vehicle_model..."
    # Try to build the vehicle_model package
    if colcon build --packages-select vehicle_model --cmake-args -DBUILD_TESTING=OFF; then
        echo "✓ vehicle_model builds successfully!"
        
        # Check if build artifacts exist
        if [ -d "build/vehicle_model" ] && [ -d "install/vehicle_model" ]; then
            echo "✓ Build artifacts created successfully"
        else
            echo "✗ Build artifacts not found"
            exit 1
        fi
    else
        echo "✗ Build failed. This might be due to missing system dependencies."
        echo "The package structure is correct, but system setup may be incomplete."
        echo "Please ensure ROS2 and all dependencies are properly installed."
        exit 1
    fi
else
    echo "⚠ colcon not available, skipping build test"
fi

echo "=== ROS2 Vehicle Model Package Test Complete ==="
echo "✓ Package structure is correct"
echo "✓ Package follows ROS2 conventions"
if command -v colcon &> /dev/null; then
    echo "✓ Package builds successfully"
fi
echo ""
echo "The 'vehicle_model' directory has been successfully converted to a ROS2 package!"